
#ifndef LQR_VMC_CONTROLLER__FIVETHPLANNER_HPP_
#define LQR_VMC_CONTROLLER__FIVETHPLANNER_HPP_


#include <array>

namespace lqr_vmc_controller
{
class fivethplanner
{
private:
    double timehorizon_; // 过程时间
    double time_{0.0}; // 规划的当前时间
    double update_rate_; // 更新频率
    bool planningflag_{false}; // 是否在规划过程中
    std::array<double, 6> fivethparam_; // 五次多项式系数
public:
    fivethplanner();

    // 计算五次多项式系数
    void getfivethParam(double firstpos, double firstvel, double firstacc, double finalpos, double finalvel, double finalacc, double timehorizon, double update_rate);
    // 启动规划
    void startplanner();
    // 获得规划位置
    double getPosition();
    // 判断是否在规划中
    bool isplanning();
};

}

#endif